﻿#include "slamrouteplanning.h"

#include "QJsonArray"
#include "QJsonObject"

SlamRoutePlanning::SlamRoutePlanning(QObject* parent) : QObject(parent) {
  //创建默认路径
  StationModel reset_statiom_model;
  reset_statiom_model.route_type = NORMAL;
  reset_statiom_model.right_angle_turn = true;
  //    reset_statiom_model.start_station    = *new Station( 0, new Point( 0, 0 ), Station::NORMAL );
  //    reset_statiom_model.target_station   = *new Station( 0, new Point( 0, 0 ), Station::NORMAL );
  Station empty = Station();  // Station(0, Point(0, 0), Station::NORMAL);
  reset_statiom_model.stations.push_back(empty);
  reset_statiom_model.stations.push_back(empty);

  SlamModel reset_slam_model;

  reset_slam_model.num = 0;
  reset_slam_model.station_model.append(reset_statiom_model);

  reset_slam_routes.append(reset_slam_model);
}

void SlamRoutePlanning::allFileReset() {
  routeInit();
  routePrint();
  obsInit();
  obsPrint();
  SlamObsInit();
  SlamObsPrint();
  chargeIdAndIpInit();
  chargeIdAndIpPrint();
  anglejudgmentInit();
  anglejudgmentPrint();
  machineMsgInit();
  machineMsgPrint();
  manInitPoseInit();
  manInitPosePrint();
  goalyawInit();
  goalyawPrint();
  backpointInit();
  backpointPrint();
  stopBarrierInit();
  stopBarrierPrint();
}
void SlamRoutePlanning::creatSlamRouteMsg() {
  QString filename = "./ini/SlamRouteMsg.csv";
  QFile Filename(filename);
  //已经存在，删除
  if (Filename.exists()) { Filename.remove(); }
  //重新创建
  Filename.setFileName(filename);
  Filename.open(QIODevice::WriteOnly | QIODevice::Append);
  QTextStream route_text_stream(&Filename);
  QString dateTime = QDateTime::currentDateTime().toString("yyyy-MM-dd hh_mm");
  if (Filename.size() == 0) {  //新文件加首行
    route_text_stream << "SlamRouteMsg"
                      << ","
                      << "dateTime:"
                      << "," << dateTime << "\r\n"
                      << "RouteNum"
                      << ","
                      << "RoutePoint"
                      << ","
                      << "RouteMsg"
                      << "\r\n";
  }
  foreach (SlamModel demo, slam_routes) {
    route_text_stream << demo.num                              //
                      << ","                                   //
                      << demo.first_station_id                 //
                      << " - "                                 //
                      << demo.last_station_id                  //
                      << ","                                   //
                      << getMachineMsg(demo.first_station_id)  //
                      << " - "                                 //
                      << getMachineMsg(demo.last_station_id)   //
                      << "\r\n";
  }
  Filename.close();
}
void SlamRoutePlanning::obsInit() {  //修改obs文件的初始化和打印
  //清空
  obs_model.clear();
  QString obsname = "./ini/obs.csv";
  QFile obsFile(obsname);
  QStringList csvList;
  csvList.clear();

  if (!obsFile.exists()) {
    qCritical() << "错误，无法找到该文件:" << obsname;
    return;
  }

  //对csv文件进行读写操作
  if (obsFile.open(QIODevice::ReadWrite)) {
    QTextStream stream(&obsFile);
    while (!stream.atEnd()) {
      csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
    }
    obsFile.close();
  } else {
    qCritical() << "错误，打开文件失败:" << obsname;
    return;
  }

  //存到obs_model
  if (csvList.size() != 0) {
    //第1行 标注routeNum   dir 标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 8) {
      qCritical() << "第1行长度错误:" << obsname;
      return;
    }
    if (valsplit0.at(0) != "slamPointid"    //
        || valsplit0.at(1) != "slamRad"     //
        || valsplit0.at(2) != "dis"         //
        || valsplit0.at(3) != "errorvalue"  //
        || valsplit0.at(4) != "front"       //
        || valsplit0.at(5) != "back"        //
        || valsplit0.at(6) != "left"        //
        || valsplit0.at(7) != "right") {    //
      qCritical() << "第1行格式错误:" << obsname;
      return;
    }
    //第2行以下 标注x  y的内容
    for (int i = 1; i < csvList.size(); i++) {
      int maxsize = slam_point.size() - 1;
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
      ObsModel bosmodel;
      bosmodel.slam_point_id = valsplit1.at(0).toInt();
      if (bosmodel.slam_point_id > maxsize)  //判断是否超出界限
      {
        qCritical() << "错误，slamObs点超出slam_point最大长度,文件" << obsname << "第" << i + 1 << "行ID值越界!"
                    << "错误点：" << bosmodel.slam_point_id << "最大限制点：" << maxsize;
        return;
      }
      //点符合开始赋值
      bosmodel.obs_point = slam_point.at(bosmodel.slam_point_id).point;
      bosmodel.slam_rad = valsplit1.at(1).toDouble();
      bosmodel.distance = valsplit1.at(2).toInt();
      bosmodel.errorvalue = valsplit1.at(3).toDouble();
      bosmodel.front = valsplit1.at(4).toInt();
      bosmodel.back = valsplit1.at(5).toInt();
      bosmodel.left = valsplit1.at(6).toInt();
      bosmodel.right = valsplit1.at(7).toInt();
      //加入到列表
      obs_model.append(bosmodel);
    }
  }
}

void SlamRoutePlanning::obsPrint() {
  qDebug() << "切换全局避障点obs.csv文件";
  foreach (ObsModel obs, obs_model) {
    qDebug() << "[slamPointId:" << obs.slam_point_id << obs.obs_point  //
             << " slamRad:" << obs.slam_rad                            //
             << " errorvalue:" << obs.errorvalue                       //
             << " 距离：" << obs.distance                              //
             << " 前避障:" << obs.front                                //
             << " 后避障:" << obs.back                                 //
             << " 左避障:" << obs.left                                 //
             << " 右避障:" << obs.right                                //
             << "]";
  }
  qDebug() << "\r\n";
}

void SlamRoutePlanning::SlamObsInit() {
  //清空
  slam_obs_mode.clear();
  QString obsname = "./ini/slamObs.csv";
  QFile obsFile(obsname);
  QStringList csvList;
  csvList.clear();

  if (!obsFile.exists()) {
    qCritical() << "错误，无法找到该文件:" << obsname;
    return;
  }

  //对csv文件进行读写操作
  if (obsFile.open(QIODevice::ReadWrite)) {
    QTextStream stream(&obsFile);
    while (!stream.atEnd()) {
      csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
    }
    obsFile.close();
  } else {
    qCritical() << "错误，打开文件失败:" << obsname;
    return;
  }

  //存到obs_model
  if (csvList.size() != 0) {
    //第1行
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 4) {
      qCritical() << "第1行长度错误:" << obsname;
      return;
    }
    if (valsplit0.at(0) != "start" || valsplit0.at(1) != "end"  //
        || valsplit0.at(2) != "front" || valsplit0.at(3) != "back") {
      qCritical() << "第1行格式错误:" << obsname;
      return;
    }
    //第2行以下
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
      SlamObsModel mode;
      mode.start = valsplit1.at(0).toInt();
      mode.end = valsplit1.at(1).toInt();
      mode.front = valsplit1.at(2).toInt();
      mode.back = valsplit1.at(3).toInt();
      //加入到列表
      slam_obs_mode.append(mode);
    }
  }
}

void SlamRoutePlanning::SlamObsPrint() {
  qDebug() << "切换激光导航避障点slamObs.csv文件";
  foreach (SlamObsModel obs, slam_obs_mode) { qDebug() << "[起点:" << obs.start << "终点：" << obs.end << "  前避障:" << obs.front << "  后避障:" << obs.back << "]"; }
  qDebug() << "\r\n";
}

void SlamRoutePlanning::chargeIdAndIpInit() {
  //充电站ip与id
  charge_id_and_ip.clear();
  QString chargename = "./ini/charge.csv";
  QFile chargeFile(chargename);
  QStringList csvList;
  csvList.clear();

  if (!chargeFile.exists()) {
    qCritical() << "错误，无法找到该文件:" << chargename;
    return;
  }
  //对csv文件进行读写操作
  if (chargeFile.open(QIODevice::ReadWrite)) {
    QTextStream stream(&chargeFile);
    while (!stream.atEnd()) { csvList.push_back(stream.readLine().replace("\"", "")); }  //保存到List当中
    chargeFile.close();
  } else {
    qCritical() << "错误，打开文件失败:" << chargename;
    return;
  }
  if (csvList.size() != 0) {
    //第1行 id   ip 标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 2) {
      qCritical() << "第1行长度错误:" << chargename;
      return;
    }
    if (valsplit0.at(0) != "id" || valsplit0.at(1) != "ip") {
      qCritical() << "第1行格式错误:" << chargename;
      return;
    }
    //第1行以下
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
      anglejudgmentid demo;
      demo.id = valsplit1.at(0).toInt();
      //加入到列表
      anglejudgment_id.append(demo);
    }
  }
}

void SlamRoutePlanning::chargeIdAndIpPrint() {
  qDebug() << "充电站id和ip对照表";
  foreach (chargeIdAndIp demo, charge_id_and_ip)  //
  {
    qDebug() << "[id:" << demo.id << "ip" << demo.ip << "]";
  }
  qDebug() << "\r\n";
}
//读取角度判断id文件
void SlamRoutePlanning::anglejudgmentInit()
{
    anglejudgment_id.clear();
    QString anglejudgmentname = "./ini/anglejudgment.csv";
    QFile anglejudgmentFile(anglejudgmentname);
    QStringList csvList;
    csvList.clear();

    if (!anglejudgmentFile.exists()) {
      qCritical() << "错误，无法找到该文件:" << anglejudgmentname;
      return;
    }
    //对csv文件进行读写操作
    if (anglejudgmentFile.open(QIODevice::ReadWrite)) {
      QTextStream stream(&anglejudgmentFile);
      while (!stream.atEnd()) { csvList.push_back(stream.readLine().replace("\"", "")); }  //保存到List当中
      anglejudgmentFile.close();
    } else {
      qCritical() << "错误，打开文件失败:" << anglejudgmentFile;
      return;
    }
    if (csvList.size() != 0) {
      //第1行 id   ip 标题
      QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
      if (valsplit0.size() < 1) {
        qCritical() << "第1行长度错误:" << anglejudgmentFile;
        return;
      }
      if (valsplit0.at(0) != "id" ) {
        qCritical() << "第1行格式错误:" << anglejudgmentFile;
        return;
      }
      //第1行以下
      for (int i = 1; i < csvList.size(); i++) {
        QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
        anglejudgmentid demo;
        demo.id = valsplit1.at(0).toInt();
        //加入到列表
        anglejudgment_id.append(demo);
      }
    }
}

void SlamRoutePlanning::anglejudgmentPrint()
{
    qDebug() << "角度判断id对照表";
    foreach (anglejudgmentid demo, anglejudgment_id)
    {
      qDebug() << "[id:" << demo.id << "]";
    }
    qDebug() << "\r\n";
}


void SlamRoutePlanning::machineMsgInit() {
  machineMsg.clear();
  QString msgFileName = "./ini/machineMsg.csv";
  QFile msgFile(msgFileName);
  QStringList csvList;
  csvList.clear();

  if (!msgFile.exists()) {
    qCritical() << "错误，无法找到该文件:" << msgFileName;
    return;
  }
  //对csv文件进行读写操作
  if (msgFile.open(QIODevice::ReadWrite)) {
    QTextStream stream(&msgFile);
    while (!stream.atEnd()) { csvList.push_back(stream.readLine().replace("\"", "")); }  //保存到List当中
    msgFile.close();
  } else {
    qCritical() << "错误，打开文件失败:" << msgFileName;
    return;
  }
  if (csvList.size() != 0) {
    //第1行 id   msg 标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 2) {
      qCritical() << "第1行长度错误:" << msgFileName;
      return;
    }

    if (valsplit0.at(0) != "id" || valsplit0.at(1) != "msg") {
      qCritical() << "第1行格式错误:" << msgFileName;
      return;
    }
    //第1行以下
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
      MachineMsg demo;
      int id = valsplit1.at(0).toInt();
      QString msg = valsplit1.at(1);
      demo.id = id;
      demo.msg = msg;
      //加入到列表
      machineMsg.append(demo);
    }
  }

  //读取endpoint.csv
  const QString end_point_fileName = "./ini/endpoint.csv";
  QFile routeFile(end_point_fileName);
  csvList.clear();
  if (!routeFile.exists()) {
    qCritical() << "错误，无法找到该文件:" << end_point_fileName;
    return;
  }
  if (!routeFile.open(QIODevice::ReadOnly)) { return; }
  QTextStream stream(&routeFile);
  while (!stream.atEnd()) { csvList.push_back(stream.readLine().replace("\"", "")); }  //保存到List当中
  routeFile.close();
  for (int i = 0; i < csvList.size(); i++) {
    if (i % 3 == 2) {
      QStringList valsplit1 = csvList.at(i - 1).split(",");  //文字
      QStringList valsplit2 = csvList.at(i).split(",");      // id
      for (int j = 0; j < valsplit2.size(); j++) {
        int id = abs(valsplit2.at(j).toInt());
        QString msg = valsplit1.at(j);
        if (id != 0 && msg != "") {
          MachineMsg demo;
          demo.id = id;
          demo.msg = msg;
          //加入到列表
          machineMsg.append(demo);
        }
      }
    }
  }
}

void SlamRoutePlanning::machineMsgPrint() {
  qDebug() << "机台中文信息";
  foreach (MachineMsg demo, machineMsg) { qDebug() << "[id:" << demo.id << " msg:" << demo.msg << "]"; }
  qDebug() << "\r\n";
}

bool SlamRoutePlanning::slamPointInit() {
  slam_point.clear();  //清空列表
  map.clear();         //清空列表
  map_enter.clear();   //清空列表
  map_exit.clear();    //清空列表

  QString filename = "./slam-point/slam_point.csv";
  QFile SlamPointFile(filename);
  QStringList csvList;
  csvList.clear();

  if (!SlamPointFile.exists()) {
    qCritical() << "错误，无法找到该文件:/slam-point/slam_point.csv";
    return false;
  }

  //对csv文件进行读写操作
  if (SlamPointFile.open(QIODevice::ReadWrite)) {
    QTextStream stream(&SlamPointFile);
    while (!stream.atEnd()) { csvList.push_back(stream.readLine().replace("\"", "")); }  //保存到List当中
    SlamPointFile.close();
  } else {
    qCritical() << "错误，打开文件失败:/slam-point/slam_point.csv";
    return false;
  }

  if (csvList.size() != 0) {
    //第0行
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 5) {
      qCritical() << "错误，slam_point.csv首行标题数小于6！";
      return false;
    }
    if (valsplit0.at(0) != "id" || valsplit0.at(1) != "x" || valsplit0.at(2) != "y" || valsplit0.at(3) != "type" || valsplit0.at(4) != "child") {
      qCritical() << "错误，slam_point.csv首行标题名称不正确！";
      return false;
    }

    //第1行以下 检查id顺序,0,1,2,3,4,5,6,...
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
      if ((i - 1) != valsplit1.at(0).toInt()) {
        qCritical() << QString("错误，slam_point.csv，第%1行有错误，当前id：%2").arg(i + 1).arg(valsplit1.at(0).toInt());
        return false;
      }
    }

    //第1行以下 标注id x  y  的内容
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串

      if (valsplit1.size() < 4) {
        qCritical() << QString("错误，slam_point.csv，第%1行有错误, (id,x,y,type,[child]), size must be >= 4");
        return false;
      }

      int id = valsplit1.at(0).toInt();
      int x = valsplit1.at(1).toInt();
      int y = valsplit1.at(2).toInt();
      Station::StationType type = static_cast<Station::StationType>(valsplit1.at(3).toInt());
      // Station::StationFunc func = static_cast<Station::StationFunc>(valsplit1.at(4).toInt());

      Station stantion{id, Point(x, y), type /*, {func}*/};

      slam_point.append(stantion);
      map[id] = {stantion, {}};
      map_enter[id] = {stantion, {}};
      map_exit[id] = {stantion, {}};

      for (int j = 4; j < valsplit1.size(); j++) {
        if (valsplit1.at(j).trimmed().isEmpty()) { continue; }

        int child;
        SlamRoutePlanning::RouteType route_type;
        getIdAndRouteType(valsplit1.at(j), child, route_type);

        if (child <= 0) {
          qCritical() << QString("错误，slam_point.csv，第%1行有错误，当前id：%2").arg(i + 1).arg(valsplit1.at(0).toInt());
          return false;
        }

        switch (route_type) {
          case SlamRoutePlanning::NORMAL: map[id].children.insert(child); break;
          case SlamRoutePlanning::ENTER: map_enter[id].children.insert(child); break;
          case SlamRoutePlanning::EXIT: map_exit[id].children.insert(child); break;
        }
      }
    }

    for (auto& node : map) {
      for (auto& child : node.second.children) {
        if (!map.containNode(child)) {
          qCritical() << QString("错误，slam_point.csv，id%1有错误：不存在id%2").arg(node.first).arg(child);
          return false;
        }
      }
    }

    for (auto& node : map_enter) {
      for (auto& child : node.second.children) {
        if (!map_enter.containNode(child)) {
          qCritical() << QString("错误，slam_point.csv，id%1有错误：不存在id%2").arg(node.first).arg(child);
          return false;
        }
      }
    }

    for (auto& node : map_exit) {
      for (auto& child : node.second.children) {
        if (!map_enter.containNode(child)) {
          qCritical() << QString("错误，slam_point.csv，id%1有错误：不存在id%2").arg(node.first).arg(child);
          return false;
        }
      }
    }
  }
  return true;
}

void SlamRoutePlanning::getIdAndRouteType(QString str, ID& id, SlamRoutePlanning::RouteType& type) {
  auto id_str = str.trimmed();

  if (id_str.startsWith('-')) {
    id = abs(id_str.toInt());
    type = SlamRoutePlanning::ENTER;
  } else if (id_str.startsWith('~')) {
    id = abs(id_str.mid(1).toInt());
    type = SlamRoutePlanning::EXIT;
  } else {
    id = abs(id_str.toInt());
    type = SlamRoutePlanning::NORMAL;
  }
}

void SlamRoutePlanning::routeInit() {
  //第一：获取slam-point里面到坐标点文件
  if (!slamPointInit()) {
    qCritical() << "错误，slam_point初始化失败，无法加载路线";
    return;
  }

  if (slam_point.size() == 0) {
    qCritical() << "错误，slam_point长度0，无法加载路线";
    return;
  }

  if (!slam_routes.isEmpty()) { slam_routes.clear(); }  //清空列表

  QDir* dir = new QDir("./slam-route");
  QStringList filter;
  filter << "*.csv";
  dir->setNameFilters(filter);  //过滤文件类型
  QList<QFileInfo>* fileInfo = new QList<QFileInfo>(dir->entryInfoList(filter));
  int count = fileInfo->count();  //文件个数

  if (count == 0) {
    qCritical() << "错误，slam-route文件夹内无路线文件";
    return;
  }

  for (int i = 1; i <= count; i++) { readSlamRouteFile(i); }
}

void SlamRoutePlanning::readSlamRouteFile(const int& num) {
  QString routename = "./slam-route/" + QString::number(num) + ".csv";
  // routeFile.setFileName( routename );
  QFile routeFile(routename);
  QStringList csvList;
  csvList.clear();
  if (!routeFile.exists()) {
    qCritical() << "错误，无法找到该路线文件:" << num << ".csv";
    return;
  }
  //对csv文件进行读写操作
  if (routeFile.open(QIODevice::ReadWrite)) {
    QTextStream stream(&routeFile);
    while (!stream.atEnd()) {
      csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
    }
    routeFile.close();
  } else {
    qCritical() << "错误，打开文件失败:" << num << ".csv";
    return;
  }

  //存到slam_routes
  if (csvList.size() != 0) {
    //第1行 标注routeNum   fun 标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 2) {
      qCritical() << "错误，" << num << ".csv路线文件首行标题数错误!";
      return;
    }
    if (valsplit0.at(0) != "routeNum" || valsplit0.at(1) != "fun") {
      qCritical() << "第1行格式错误:" << num << ".csv";
      return;
    }

    //第2行 标注routeNum  fun  的内容
    QStringList valsplit1 = csvList.at(1).split(",");  //分隔字符串
    SlamModel slam_model;
    slam_model.num = valsplit1.at(0).toInt();
    slam_model.fun_model = static_cast<FunModel>(valsplit1.at(1).toUInt());
    //第3行 标注start  end  dir标题
    QStringList valsplit2 = csvList.at(2).split(",");  //分隔字符串
    if (valsplit2.size() < 3) {
      qCritical() << "错误，" << num << ".csv路线文件第3行标题数错误!";
      return;
    }
    if (valsplit2.at(0) != "start" || valsplit2.at(1) != "end" || valsplit2.at(2) != "type") {
      qCritical() << "第3行格式错误:" << num << ".csv";
      return;
    }

    //第4行以下 标注start  end  dir的内容
    for (int i = 3; i < csvList.size(); i++) {
      QStringList valsplit3 = csvList.at(i).split(",");  //分隔字符串
      int start = valsplit3.at(0).toInt();
      QStringList end = valsplit3.at(1).split("=");
      int maxsize = slam_point.size() - 1;
      if (start > maxsize) {  // 判断是否超出界限
        qCritical() << "错误，路线点超出slam_point最大长度,路线文件" << num << ".csv第" << i + 1 << "行ID值越界!"
                    << "起点：" << start << "终点：" << end << "最大点：" << maxsize;
        return;
      }
      StationModel station_model;
      station_model.stations.push_back(slam_point.at(start));
      for (auto str : end) {
        int id = str.toInt();

        if (id > maxsize) {  //判断是否超出界限
          qCritical() << "错误，路线点超出slam_point最大长度,路线文件" << num << ".csv第" << i + 1 << "行ID值越界!"
                      << "起点：" << start << "终点：" << end << "最大点：" << maxsize;
          return;
        }

        station_model.stations.push_back(slam_point.at(str.toInt()));
      }

      switch (valsplit3.at(2).toUInt()) {
        case 1:
          station_model.route_type = SlamRoutePlanning::ENTER;
          station_model.isforward = true;
          break;
        case 2:
          station_model.route_type = SlamRoutePlanning::EXIT;
          station_model.isforward = false;
          break;
        default:
          station_model.route_type = SlamRoutePlanning::NORMAL;
          station_model.isforward = true;
          break;
      }

      //      station_model.isforward = static_cast<bool>(valsplit3.at(2).toUInt());
      //      station_model.right_angle_turn = static_cast<bool>(valsplit3.at(3).toUInt());
      //      station_model.station_fun = static_cast<StationFunModel>(valsplit3.at(4).toUInt());
      if (i == 3) { slam_model.first_station_id = station_model.stations.front().id; }                 //记录整条路线的起点
      if (i == csvList.size() - 1) { slam_model.last_station_id = station_model.stations.back().id; }  //记录整条路线的终点
      slam_model.station_model.append(station_model);
    }
    //加入到列表
    slam_routes.append(slam_model);
  }
  //重对接路线
  // redoRouteInit();
}

void SlamRoutePlanning::manInitPoseInit() {
  //清空
  man_init_pose.clear();
  QString name = "./ini/manInitPos.csv";
  QFile File(name);
  QStringList csvList;
  csvList.clear();

  if (!File.exists()) {
    qCritical() << "错误，无法找到该文件:" << name;
    return;
  }

  //对csv文件进行读写操作
  if (File.open(QIODevice::ReadWrite)) {
    QTextStream stream(&File);
    while (!stream.atEnd()) {
      csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
    }
    File.close();
  } else {
    qCritical() << "错误，打开文件失败:" << name;
    return;
  }

  //存到model
  if (csvList.size() != 0) {
    //第1行  标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 2) {
      qCritical() << "第1行长度错误:" << name;
      return;
    }
    if (valsplit0.at(0) != "id" || valsplit0.at(1) != "th") {
      qCritical() << "第1行格式错误:" << name;
      return;
    }
    //第2行以下 标注x  y的内容
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串
      ManInitPose model;
      model.id = valsplit1.at(0).toInt();
      if (model.id > (slam_point.size() - 1)) {
        qCritical() << name << "文件错误，"
                    << "设定的重定位id:" << model.id << ",超出slam_point范围";
        return;
      }
      Point point;
      point = slam_point.at(model.id).point;
      point.setRadian(valsplit1.at(1).toDouble());
      model.point = point;
      //加入到列表
      man_init_pose.append(model);
    }
  }
}

void SlamRoutePlanning::manInitPosePrint() {
  qDebug() << "重定位点位姿";
  foreach (ManInitPose model, man_init_pose) { qDebug() << "[id:" << model.id << " point:" << model.point << " th:" << model.point.radian() << "]"; }
  qDebug() << "\r\n";
}

void SlamRoutePlanning::goalyawInit() {  //清空
  goal_yaw.clear();
  QString name = "./ini/goalYaw.csv";
  QFile File(name);
  QStringList csvList;
  csvList.clear();

  if (!File.exists()) {
    qCritical() << "错误，无法找到该文件:" << name;
    return;
  }

  //对csv文件进行读写操作
  if (File.open(QIODevice::ReadWrite)) {
    QTextStream stream(&File);
    while (!stream.atEnd()) {
      csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
    }
    File.close();
  } else {
    qCritical() << "错误，打开文件失败:" << name;
    return;
  }

  //存到model
  if (csvList.size() != 0) {
    //第1行  标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 2) {
      qCritical() << "第1行长度错误:" << name;
      return;
    }
    if (valsplit0.at(0) != "id" || valsplit0.at(1) != "yaw") {
      qCritical() << "第1行格式错误:" << name;
      return;
    }
    //第2行以下 标注x  y的内容
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串

      goalYaw model;
      model.id = valsplit1.at(0).toInt();
      model.yaw = valsplit1.at(1).toDouble();
      //加入到列表
      goal_yaw.append(model);
    }
  }
}

void SlamRoutePlanning::goalyawPrint() {
  qDebug() << "终点角度";
  foreach (goalYaw model, goal_yaw) { qDebug() << "[id:" << model.id << " yaw:" << model.yaw << "]"; }
  qDebug() << "\r\n";
}

void SlamRoutePlanning::backpointInit()
{
    back_point.clear();
    QString name = "./ini/BackPoint.csv";
    QFile File(name);
    QStringList csvList;
    csvList.clear();

    if (!File.exists()) {
      qCritical() << "错误，无法找到该文件:" << name;
      return;
    }

    //对csv文件进行读写操作
    if (File.open(QIODevice::ReadWrite)) {
      QTextStream stream(&File);
      while (!stream.atEnd()) {
        csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
      }
      File.close();
    } else {
      qCritical() << "错误，打开文件失败:" << name;
      return;
    }
    //存到model
    if (csvList.size() != 0) {
      //第1行  标题
      QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
      if (valsplit0.size() < 3) {
        qCritical() << "第1行长度错误:" << name;
        return;
      }
      if (valsplit0.at(0) != "id" || valsplit0.at(1) != "num" || valsplit0.at(2) != "speed" ) {
        qCritical() << "第1行格式错误:" << name;
        return;
      }
      //第2行以下 标注x  y的内容
      for (int i = 1; i < csvList.size(); i++) {
        QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串

        backpoint model;
        model.id = valsplit1.at(0).toInt();
        model.num = valsplit1.at(1).toInt();
        model.speed = valsplit1.at(2).toDouble();
        //加入到列表
        back_point.append(model);
      }
    }

}

void SlamRoutePlanning::backpointPrint()
{
    qDebug() << "后退ID点位和速度";
    foreach (backpoint model, back_point) { qDebug() << "[id:" << model.id << " num:" << model.num << " speed:" << model.speed << "]"; }
    qDebug() << "\r\n";
}

void SlamRoutePlanning::stopBarrierInit() {
  stop_barrier.clear();
  QString name = "./ini/stopBarrier.csv";
  QFile File(name);
  QStringList csvList;
  csvList.clear();

  if (!File.exists()) {
    qCritical() << "错误，无法找到该文件:" << name;
    return;
  }

  //对csv文件进行读写操作
  if (File.open(QIODevice::ReadWrite)) {
    QTextStream stream(&File);
    while (!stream.atEnd()) {
      csvList.push_back(stream.readLine().replace("\"", ""));  //保存到List当中
    }
    File.close();
  } else {
    qCritical() << "错误，打开文件失败:" << name;
    return;
  }

  //存到model
  if (csvList.size() != 0) {
    //第1行  标题
    QStringList valsplit0 = csvList.at(0).split(",");  //分隔字符串
    if (valsplit0.size() < 4) {
      qCritical() << "第1行长度错误:" << name;
      return;
    }
    if (valsplit0.at(0) != "fun" || valsplit0.at(1) != "start" || valsplit0.at(2) != "end" || valsplit0.at(3) != "dis") {
      qCritical() << "第1行格式错误:" << name;
      return;
    }
    //第2行以下 标注x  y的内容
    for (int i = 1; i < csvList.size(); i++) {
      QStringList valsplit1 = csvList.at(i).split(",");  //分隔字符串

      stopBarrier model;
      model.fun = valsplit1.at(0).toInt();
      model.start = valsplit1.at(1).toInt();
      model.end = valsplit1.at(2).toInt();
      model.dis = valsplit1.at(3).toInt();
      //加入到列表
      stop_barrier.append(model);
    }
  }
}

void SlamRoutePlanning::stopBarrierPrint() {
  qDebug() << "停障选择";
  foreach (stopBarrier model, stop_barrier) { qDebug() << "[fun:" << model.fun << " start:" << model.start << " end:" << model.end << " dis:" << model.dis << "]"; }
  qDebug() << "\r\n";
}

QDebug operator<<(QDebug debug, const std::set<ID>& s) {
  debug.noquote().nospace() << "[";
  for (ID id : s) { debug.noquote().nospace() << QString::number(id) << ","; }
  debug.noquote().nospace() << "]";
  return debug;
}

/**
 * @brief SlamRoutePlanning::print 打印路线信息
 */
void SlamRoutePlanning::routePrint() {
  qDebug() << "激光点文件";
  foreach (Station slamPoint, slam_point) {  //打印slam_point
    qDebug() << slamPoint << ",normal:" << map[slamPoint.id].children << ",enter:" << map_enter[slamPoint.id].children << ",exit:" << map_exit[slamPoint.id].children;
  }
  qDebug() << "\r\n";

  qDebug() << "激光路线文件";
  foreach (SlamModel slam_mode, slam_routes) {
    qDebug() << "[线路号:" << slam_mode.num << "功能:" << slam_mode.fun_model << "起点:" << slam_mode.first_station_id << "终点:" << slam_mode.last_station_id << "]";
    foreach (StationModel station_model, slam_mode.station_model) {  //打印没段坐标与方向
      qDebug() << "turn:" << station_model.right_angle_turn << "type:" << station_model.route_type << station_model.stations.front() << "->" << station_model.stations.back();
    }
    qDebug() << "\r\n";
  }

  // redoRoutePrint();
}

/*void SlamRoutePlanning::redoRouteInit() {
    if ( slam_routes.isEmpty() )  //清空列表
    {
        return;
    }
    if ( !redo_slam_routes.isEmpty() )  //清空列表
    {
        redo_slam_routes.clear();
    }
    //做redo的路线
    foreach ( SlamModel slam_model, slam_routes ) {
        int size = slam_model.station_model.size() - 1;
        if ( slam_model.fun_model == SWITCH_NAVI && ( size >= 0 ) ) {
            SlamModel redo_slam_model;
            redo_slam_model.num       = slam_model.num;
            redo_slam_model.fun_model = SWITCH_NAVI;

            StationModel redo_station_model;

            redo_station_model.start_station    = slam_model.station_model.at( size ).target_station;
            redo_station_model.target_station   = slam_model.station_model.at( size ).start_station;
            redo_station_model.isforward        = !slam_model.station_model.at( size ).isforward;
            redo_station_model.right_angle_turn = 1;
            redo_slam_model.station_model.append( redo_station_model );
            redo_station_model.start_station    = slam_model.station_model.at( size ).start_station;
            redo_station_model.target_station   = slam_model.station_model.at( size ).target_station;
            redo_station_model.isforward        = slam_model.station_model.at( size ).isforward;
            redo_station_model.right_angle_turn = 1;
            redo_slam_model.station_model.append( redo_station_model );
            redo_slam_routes.append( redo_slam_model );
        }
    }
}*/

/*void SlamRoutePlanning::redoRoutePrint() {
    qDebug() << "激光重对接路线文件";
    foreach ( SlamModel slam_mode, redo_slam_routes ) {
        qDebug() << "[重对接线路号:" << slam_mode.num << "功能:" << slam_mode.fun_model << "]";
        foreach ( StationModel station_model, slam_mode.station_model )  //打印没段坐标与方向
        {
            qDebug() << "turn:" << station_model.right_angle_turn << "dir:" << station_model.isforward << station_model.start_station << "->" <<
station_model.target_station;
        }
        qDebug() << "\r\n";
    }
}*/

QList<SlamRoutePlanning::StationModel> SlamRoutePlanning::getStationModel(const int& num) {
  for (int i = 0; i < slam_routes.size(); i++) {
    if (num == slam_routes.at(i).num) {
      return slam_routes.at(i).station_model;  // slam_routes.at(0).stations.at(0)->point()->x();
    }
  }
  qCritical() << "错误，无法找到该路线模型getStationMode:" << QString::number(num) << "号路线";
  return reset_slam_routes.at(0).station_model;  //加默认值
}

QList<SlamRoutePlanning::StationModel> SlamRoutePlanning::caluStatuonModel(const int& start, const int& goal, RouteType route_type) {
  Map& check_map = route_type == ENTER ? map_enter : route_type == EXIT ? map_exit : map;

  if (!check_map.containNode(start)) {
    qCritical() << "错误，不存在的起始点，无法计算该路线模型caluStatuonModel: 路线" << QString::number(start) << "->" << QString::number(goal);
    return {};
  }

  if (!check_map.containNode(goal)) {
    qCritical() << "错误，不存在的目标点，无法计算该路线模型caluStatuonModel: 路线" << QString::number(start) << "->" << QString::number(goal);
    return {};
  }

  if (start == goal) {
    qCritical() << "错误，起始点与目标点相同，无法计算该路线模型caluStatuonModel: 路线" << QString::number(start) << "->" << QString::number(goal);
    return {};
  }

  Path path;
  if (!Dijkstra().findPath(check_map, start, goal, path)) {
    qCritical() << "错误，目标点无法到达，无法计算该路线模型caluStatuonModel: 路线" << QString::number(start) << "->" << QString::number(goal);
    return {};
  }

  if (path.size() < 2) {
    qCritical() << "错误，未知错误，无法计算该路线模型caluStatuonModel: 路线" << QString::number(start) << "->" << QString::number(goal);
    return {};
  }

  // 简化路径 剔除可忽略的点
  Path simplify_path;
  bool simplify_flag = false;
  for (Path::reverse_iterator it = path.rbegin(); it != path.rend(); it++) {
    if (simplify_flag) {
      Path::reverse_iterator next = std::next(it);
      if (next != path.rend() && check_map[*next].station.type == Station::CAN_BE_SIMPLIFIED) {
        // ignore if current on line(last, next)
        // qDebug()<<Utility::calcDisPA2LBC(check_map[*it].station.point, check_map[simplify_path.back()].station.point, check_map[*next].station.point);
        if (Utility::calcDisPA2LBC(check_map[*it].station.point, check_map[simplify_path.back()].station.point, check_map[*next].station.point) < 300) { continue; }
      } else {
        simplify_flag = false;
      }
    } else {
      if (check_map[*it].station.type == Station::CAN_BE_SIMPLIFIED) { simplify_flag = true; }
    }

    simplify_path.push_back(*it);
  }

  //  // 带功能的点位，将分为多段路线
  //  std::vector<Path> paths;
  //  paths.push_back({});
  //  for (Path::iterator it = std::next(simplify_path.begin()); it != simplify_path.end(); it++) {
  //    if (!check_map[*it].station.func.empty()) {
  //      paths.back().push_back(*it);
  //      if (std::next(it) == simplify_path.end()) { break; }
  //      paths.push_back({});
  //    }
  //    paths.back().push_back(*it);
  //  }

  QList<SlamRoutePlanning::StationModel> models;

  StationModel model;
  model.route_type = route_type;
  switch (model.route_type) {
    case SlamRoutePlanning::NORMAL: model.isforward = true; break;
    case SlamRoutePlanning::ENTER: model.isforward = false; break;
    case SlamRoutePlanning::EXIT: model.isforward = true; break;
  }
  model.station_fun = GOON;
  for (auto id : simplify_path) {
    if (!check_map.containNode(id)) {
      qCritical() << "错误，站点信息有误，不存在的站点" << id << "，caluStatuonModel:" << QString::number(start) << "->" << QString::number(goal) << "路线";
      return {};
    }
    model.stations.push_back(check_map[id].station);
  }
  models.push_back(model);

  return models;
}

/*QList< SlamRoutePlanning::StationModel > SlamRoutePlanning::getRedoStationMode( const int num ) {
    for ( int i = 0; i < redo_slam_routes.size(); i++ ) {
        if ( num == redo_slam_routes.at( i ).num ) {
            return redo_slam_routes.at( i ).station_model;  //     slam_routes.at(0).stations.at(0)->point()->x();
        }
    }
    qCritical() << "错误，无法找到该路线模型getRedoStationMode:" << QString::number( num ) << "号路线";
    return reset_slam_routes.at( 0 ).station_model;  //加默认值
}*/

SlamRoutePlanning::FunModel SlamRoutePlanning::getFunMode(const int& num) {
  for (int i = 0; i < slam_routes.size(); i++) {
    if (num == slam_routes.at(i).num) {
      return slam_routes.at(i).fun_model;  //     slam_routes.at(0).stations.at(0)->point()->x();
    }
  }
  // qCritical() << "错误，无法找到该功能模型getFunMode:" << QString::number( num ) << "号路线";
  return STANDBY;  //加默认值
}

QString SlamRoutePlanning::getFunModeMsg(const int& num) {
  switch (getFunMode(num)) {
    case SlamRoutePlanning::STANDBY: return "待命";
    case SlamRoutePlanning::ALLOW_TRANSMIT: return "对接";
    default: return "未知";
  }
}

bool SlamRoutePlanning::isSlamRoutes(const int& num) {
  for (int i = 0; i < slam_routes.size(); i++) {
    if (num == slam_routes.at(i).num) { return true; }
  }
  return false;
}

bool SlamRoutePlanning::isGoalVaild(const int& id) {
  foreach (auto machine, machineMsg) {
    if (machine.id == id) { return true; }
  }
  return false;
}

bool SlamRoutePlanning::isIdVaild(const int& id) { return map.containNode(id); }

//修改obs判断避障切换
bool SlamRoutePlanning::isNeedChangeObs(const Point& curPoint, int* front, int* back, bool* Lobs, bool* Robs) {
  ObsModel callBackObs;
  int obsMinDistance = 999999;
  foreach (ObsModel obs, obs_model) {
    int dis = static_cast<int>(Utility::calcDistance(curPoint, obs.obs_point));
    double rad_err = curPoint.radian() - obs.slam_rad;
    Utility::radianCorrection(&rad_err);
    if (dis <= obs.distance && fabs(rad_err) < obs.errorvalue) {  //进入范围圈，并且角度在+-15度符合范围
      if (obs.distance < obsMinDistance) {                        //找整个list里面半径设定最小的
        obsMinDistance = obs.distance;
        callBackObs = obs;
      }
    }
  }
  if (obsMinDistance != 999999) {  //已经找到符合条件
    *front = callBackObs.front;
    *back = callBackObs.back;
    *Lobs = callBackObs.left == 1 ? true : false;
    *Robs = callBackObs.right == 1 ? true : false;
    return true;
  }
  return false;
}

bool SlamRoutePlanning::isNeedChangeSlamObs(const int& start, const int& end, int* front, int* back) {
  foreach (SlamObsModel obs, slam_obs_mode) {
    if (start == obs.start && end == obs.end) {
      *front = obs.front;
      *back = obs.back;
      return true;
    }
  }
  return false;
}

/**
 * @brief SlamRoutePlanning::getSlamRouteSum 获取路线总数
 * @return 路线总数
 */
int SlamRoutePlanning::getSlamRouteSum() const { return slam_routes.size(); }

int SlamRoutePlanning::getSlamPointId(const Point& curPoint) {
  int id = 0, result = 999999, minLimitDis = 300;
  foreach (Station station, slam_point) {
    int dis = static_cast<int>(Utility::calcDistance(curPoint, station.point));
    //点位距离过于接近，无法用原来到满足条件（小于300）就return结果，使用minLimitDis找到最小距离合适的id方可使用
    if ((dis <= 300) && (dis <= minLimitDis)) {
      minLimitDis = dis;
      id = station.id;
      result = id;
    }
  }
  return result;
}

bool SlamRoutePlanning::findResentSlamPoint(const Point& curPoint, ID& id, int& min_dist) {
  id = -1, min_dist = -1;
  foreach (Station station, slam_point) {
    if (station.id < 1) { continue; }
    int dis = static_cast<int>(Utility::calcDistance(curPoint, station.point));
    if (min_dist < 0 || dis <= min_dist) {
      min_dist = dis;
      id = station.id;
    }
  }
  return id > 0 && min_dist >= 0;
}

bool SlamRoutePlanning::getSlamPointValue(const int& id, Point& curPoint) {
  foreach (Station station, slam_point) {
    if (id == station.id) {
      curPoint = station.point;
      return true;
    }
  }
  return false;
}

QString SlamRoutePlanning::getChargeIp(const int& id) {
  foreach (chargeIdAndIp demo, charge_id_and_ip) {
    if (demo.id == id) { return demo.ip; }
  }
  // qCritical() << "错误，无法找到该充电站对应的IP，ID：" << id;
  return "";
}

int SlamRoutePlanning::getSlamRouteFirstId(const int& num) {
  foreach (SlamModel slam_mode, slam_routes) {
    if (slam_mode.num == num) { return slam_mode.first_station_id; }
  }
  return 0;
}

int SlamRoutePlanning::getSlamRouteLastId(const int& num) {
  foreach (SlamModel slam_mode, slam_routes) {
    if (slam_mode.num == num) { return slam_mode.last_station_id; }
  }
  return 0;
}

QList<int> SlamRoutePlanning::getSlamRoute(const int& lastId) {
  QList<int> route;
  route.clear();
  foreach (SlamModel slam_mode, slam_routes) {
    if (slam_mode.last_station_id == lastId) { route.append(slam_mode.num); }
  }
  return route;
}

//  充电id的判断
bool SlamRoutePlanning::isChargeNum(const int& id) {
  foreach (chargeIdAndIp demo, charge_id_and_ip) {
    if (demo.id == id) { return true; }
  }
  return false;
}
//角度判断id
bool SlamRoutePlanning::isAnglejudgment(const int &id)
{
    foreach( anglejudgmentid demo1, anglejudgment_id  ){
        if(demo1.id == id){return true; }
    }
    return  false;
}

bool SlamRoutePlanning::getManInitPosePoint(const int& num, Point& curPoint) {
  foreach (ManInitPose demo, man_init_pose) {
    if (demo.id == num) {
      curPoint = demo.point;
      return true;
    }
  }
  return false;
}

bool SlamRoutePlanning::getgoalYaw(const int& num, double& yaw) {
  foreach (goalYaw demo, goal_yaw) {
    if (demo.id == num) {
      yaw = demo.yaw;
      return true;
    }
  }
  return false;
}

bool SlamRoutePlanning::getbackpoint(const int &id, int &num, double &speed)
{
    foreach (backpoint demo, back_point) {
      if (demo.id == id) {
        num = demo.num;
        speed = demo.speed;
        return true;
      }
    }
    return false;
}

bool SlamRoutePlanning::isNeedStopBarrier(int fun, const int& start, const int& end, const double& start2targetdis, const double& cur2targetdis) {
  foreach (stopBarrier demo, stop_barrier) {
    if (demo.fun == fun && demo.start == start && demo.end == end) {
      if (fun == 1)  //要触发停障，还需要再判断距离
      {
        // demo.dis距离值在车起步前和最后该距离值内不需要停障
        if (((start2targetdis - cur2targetdis) > demo.dis) && (cur2targetdis > demo.dis)) return true;
      } else  //直接绕行的，路线段符合即可
      {
        return true;
      }
    }
  }
  return false;
}


QString SlamRoutePlanning::getMachineMsg(const int& num) {
  foreach (MachineMsg demo, machineMsg) {
    if (demo.id == num) return demo.msg;
  }
  return "空";
}
